﻿using System;
using System.IO.Ports;
using System.Threading;

namespace CAPTS
{
    public class StepperMotorController : IDisposable
    {
        private SerialPort _serialPort;
        private readonly object _lock = new object();
        private const int Timeout = 500; // 毫秒
        private const int MaxRetries = 3; // 最大重试次数

        public StepperMotorController(string portName, int baudRate = 115200)
        {
            _serialPort = new SerialPort(portName, baudRate)
            {
                Parity = Parity.None,
                DataBits = 8,
                StopBits = StopBits.One,
                ReadTimeout = Timeout,
                WriteTimeout = Timeout
            };
        }

        public void Open()
        {
            if (!_serialPort.IsOpen)
            {
                _serialPort.Open();
                Thread.Sleep(100); // 等待端口稳定
            }
        }

        public void Close()
        {
            if (_serialPort.IsOpen)
            {
                _serialPort.Close();
            }
        }

        #region 核心通信方法
        private bool WriteRegisterWithResponse(byte deviceAddress, ushort registerAddress, ushort value)
        {
            byte[] command = CreateModbusCommand(deviceAddress, 0x06, registerAddress, value);
            byte[] expectedResponse = command; // 正确响应应与发送的命令相同

            for (int retry = 0; retry < MaxRetries; retry++)
            {
                try
                {
                    lock (_lock)
                    {
                        _serialPort.DiscardInBuffer();
                        _serialPort.Write(command, 0, command.Length);

                        byte[] response = new byte[8];
                        int bytesRead = ReadResponse(response, Timeout);

                        if (bytesRead == 8 && ByteArrayEquals(response, expectedResponse))
                        {
                            return true;
                        }
                    }
                }
                catch { /* 忽略错误进行重试 */ }
                Thread.Sleep(10 * (retry + 1)); // 递增重试延迟
            }
            return false;
        }

        private int ReadResponse(byte[] buffer, int timeout)
        {
            int bytesRead = 0;
            DateTime start = DateTime.Now;

            while (bytesRead < buffer.Length && (DateTime.Now - start).TotalMilliseconds < timeout)
            {
                if (_serialPort.BytesToRead > 0)
                {
                    bytesRead += _serialPort.Read(buffer, bytesRead, buffer.Length - bytesRead);
                }
                Thread.Sleep(1);
            }
            return bytesRead;
        }

        private byte[] CreateModbusCommand(byte deviceAddress, byte functionCode, ushort registerAddress, ushort value)
        {
            byte[] command = new byte[8];
            command[0] = deviceAddress;
            command[1] = functionCode;
            command[2] = (byte)(registerAddress >> 8);
            command[3] = (byte)registerAddress;
            command[4] = (byte)(value >> 8);
            command[5] = (byte)value;

            ushort crc = CalculateCRC(command, 6);
            command[6] = (byte)crc;
            command[7] = (byte)(crc >> 8);

            return command;
        }

        private bool ByteArrayEquals(byte[] a1, byte[] a2)
        {
            if (a1.Length != a2.Length) return false;
            for (int i = 0; i < a1.Length; i++)
            {
                if (a1[i] != a2[i]) return false;
            }
            return true;
        }
        #endregion

        #region 电机控制方法
        public bool SetDirection(byte deviceAddress, Direction direction, bool verify = true)
        {
            if (verify)
            {
                return WriteRegisterWithResponse(deviceAddress, 0x009B, (ushort)direction);
            }
            else
            {
                WriteRegister(deviceAddress, 0x009B, (ushort)direction);
                return true;
            }
        }

        public bool SetPulseCount(byte deviceAddress, uint pulseCount, bool verify = true)
        {
            bool success = true;

            // 设置高16位
            if (verify)
                success &= WriteRegisterWithResponse(deviceAddress, 0x009C, (ushort)(pulseCount >> 16));
            else
                WriteRegister(deviceAddress, 0x009C, (ushort)(pulseCount >> 16));

            // 设置低16位
            if (verify)
                success &= WriteRegisterWithResponse(deviceAddress, 0x009D, (ushort)(pulseCount & 0xFFFF));
            else
                WriteRegister(deviceAddress, 0x009D, (ushort)(pulseCount & 0xFFFF));

            return success;
        }

        public void StopAll(bool verify = false)
        {
            for (byte i = 1; i <= 64; i++)
            {
                SetDirection(i, Direction.Stop, verify);
            }
        }

        public bool MultiCommand(byte deviceAddress, Action<StepperMotorController>[] commands, bool verify = true)
        {
            bool allSuccess = true;
            foreach (var cmd in commands)
            {
                // 为每个命令创建一个临时控制器实例
                var tempController = new StepperMotorController(_serialPort.PortName, _serialPort.BaudRate)
                {
                    _serialPort = this._serialPort // 共享同一个串口实例
                };

                cmd(tempController);

                if (verify)
                {
                    // TODO: 添加特定验证逻辑
                    allSuccess &= VerifyLastCommand(deviceAddress);
                }
            }
            return allSuccess;
        }
        #endregion

        #region 
        private ushort CalculateCRC(byte[] buffer, int length)
        {
            ushort crc = 0xFFFF;
            for (int pos = 0; pos < length; pos++)
            {
                crc ^= buffer[pos];
                for (int i = 8; i != 0; i--)
                {
                    if ((crc & 0x0001) != 0)
                    {
                        crc >>= 1;
                        crc ^= 0xA001;
                    }
                    else
                    {
                        crc >>= 1;
                    }
                }
            }
            return crc;
        }

        private bool VerifyLastCommand(byte deviceAddress)
        {
            // TODO: 实现具体验证逻辑
            // 可以读取状态寄存器验证最后一条命令是否生效
            Thread.Sleep(20); // 等待设备响应
            return true;
        }

        private void WriteRegister(byte deviceAddress, ushort registerAddress, ushort value)
        {
            byte[] command = CreateModbusCommand(deviceAddress, 0x06, registerAddress, value);
            lock (_lock)
            {
                _serialPort.Write(command, 0, command.Length);
                Thread.Sleep(10); // 命令间隔
            }
        }
        #endregion

        public void Dispose()
        {
            StopAll();
            Close();
            _serialPort?.Dispose();
        }
    }

    public enum Direction : ushort
    {
        Stop = 0,
        Clockwise = 1,    // 正转
        CounterClockwise = 2 // 反转
    }
}